#ifndef _SK_H
#define _SK_H    1

#include "utils.h"

struct URMES {
	int mesize; //结构体中int与double对齐,占用8个字节
	double time;
	double q_target[6];
	double qd_target[6];
	double qdd_target[6];
	double i_target[6];
	double m_target[6];
	double q_actual[6];
	double qd_actual[6];
	double i_actual[6];
	double i_control[6];
	double tcp_actual[6];
	double tcp_speed_actual[6];
	double tcp_force[6];
	double tcp_target[6];
	double tcp_speed_target[6];
};

/* 全局变量 */
int sockfd;
struct URMES urmes;

double ntohd64(char *p)
{
	union {
		double d64;
		char mem[8];
	} bswap;
	for (int i=7; i>=0; i--)
	{
		bswap.mem[i] = *(p++);
	}
	return bswap.d64;
}

void ntohd64_6(double *p, char *q)
{
	for (int i=0; i<6; i++)
	{
		*(p+i) = ntohd64(q+8*i);
	}
}

void recvmes(int sockfd, struct URMES *mes)
{
	char buf[1140];
	memset(buf, 0, sizeof(buf));
	struct URMES urmes;
	memset(&urmes, 0, sizeof(urmes));
	if (recv(sockfd, buf, sizeof(buf), 0) < 0)
	{
		printf("recvmsg error: %s(errno: %d)\n", strerror(errno), errno);
	}
	else
	{
		//Message Size
		uint32_t q;
		memcpy(&q, &buf[0], 4);
		urmes.mesize = ntohl(q);
		//Time
		urmes.time = ntohd64(buf+4);
		//q target ~ TCP speed target
		for (int i=0; i<14; i++)
		{
			ntohd64_6(urmes.q_target+i*6, buf+12+48*i);
		}
	}
	*mes = urmes;
}

int sendcmd(const char* cmdstr)
{
	printf("%s", cmdstr);
	if (send(sockfd, cmdstr, strlen(cmdstr), 0) < -1)
	{
		printf("sendcmd error: %s(errno: %d)\n", strerror(errno), errno);
		return -1;
	}
	return 0;
}

int closesk(int sockfd)
{
	if (close(sockfd) < 0)
	{
		printf("close error: %s(errno: %d)\n", strerror(errno), errno);
		return -1;
	}
	return 0;
}

void *RecvMes()
{
	while (1) recvmes(sockfd, &urmes);
}

/* 全局变量, 标记是否记录 */
int g_record = 0;

void *BackProcess()
{
	FILE *fp = fopen("actual_move_path.txt", "w");

	while (1)
	{
		if (g_record == 1 && threshold(urmes.tcp_actual[2]-HEIGHT*0.001))
		{
			fprintf(fp, "%lf %lf %lf\n", urmes.tcp_actual[0], urmes.tcp_actual[1], urmes.tcp_actual[2]);
		}
		usleep(1e6*0.032);
	}
}

void ConnectUR()
{
	//创建套接字
	if ((sockfd = socket(AF_INET, SOCK_STREAM, 0)) < 0)
	{
		printf("create socket error: %s(errno: %d)\n", strerror(errno), errno);
		exit(-1);
	}

	//配置地址和端口
	struct sockaddr_in servaddr;
	memset(&servaddr, 0, sizeof(servaddr));

	servaddr.sin_family = AF_INET;
	servaddr.sin_port = htons(30003);
	servaddr.sin_addr.s_addr = inet_addr("192.168.56.101");

	//连接服务端
	if (connect(sockfd, (struct sockaddr*)&servaddr, sizeof(servaddr)) < 0)
	{
		printf("connect error: %s(errno: %d)\n", strerror(errno), errno);
		exit(-1);
	}

	//初始化消息结构体
	memset(&urmes, 0, sizeof(urmes));

	//创建消息接收线程
	pthread_t th;
	pthread_create(&th, NULL, RecvMes, NULL);

	//创建后台处理进程
	pthread_t th2;
	pthread_create(&th2, NULL, BackProcess, NULL);

	//分离线程
	pthread_detach(th);
	pthread_detach(th2);
}

#endif /* <sk.h> included.  */